"""
@作者:sipeiliu@163.com

@节点身份：face_result服务的客户端，camera_status动作的客户端

@节点任务：人机交互
"""

# ros2 库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from rclpy.action import ActionClient # 动作客户端库

# 其它库
import time # 时间库

# 导入消息类型
from facer_interfaces.srv import FaceResult # 自定义检测结果消息类型
from facer_interfaces.action import CameraState # 自定义摄像机状态消息类型

"""人机交互节点类"""
class VisualInteraction(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('visual_interaction') # ros2节点父类初始化,节点名称为visual_interaction
          
        '''服务face_result客户端的相关代码'''       
        self.cli= self.create_client(FaceResult, 'face_result') # 创建服务客户端对象(消息类型、服务名)     
        while not self.cli.wait_for_service(timeout_sec=1.0): # 每隔一秒检查服务器是否运行,若没有运行就等待
            print('等待服务face_result的服务端运行...')       
        self.req = FaceResult.Request() # 创建服务消息类型对象
        
        '''动作camera_state客户端的相关代码'''
        self._action_client = ActionClient(self, CameraState, 'camera_state') # 创建动作客户端对象(消息类型、动作名)

    '''发送服务请求函数'''
    def send_request(self, apply_result):
        self.req.apply_result = apply_result # 设置申请结果为真，让服务端判断为可以返回检测结果
        self.future = self.cli.call_async(self.req) # 异步方式发送服务请求
        rclpy.spin_until_future_complete(self, self.future) # 阻塞直到self.future完成
        return self.future.result() # 返回结果

    '''发送动作目标函数'''
    def send_goal(self, start):
        goal_msg = CameraState.Goal() # 实例化动作的目标
        goal_msg.start = start # 给实例化的动作目标赋值
        while not self._action_client.wait_for_server(timeout_sec=1.0):
            print('等待动作camera_state的服务端运行...')
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) # 返回动作目标的future
        self._send_goal_future.add_done_callback(self.goal_reponse_callback) # 通过动作目标的future给动作目标添加回调函数

    '''动作目标的回调函数'''
    def goal_reponse_callback(self, future):
        goal_handle = future.result() # 目标处理
        if goal_handle.accepted:
            print('目标请求成功')
        else:
            print('目标请求失败')

        self._get_result_future = goal_handle.get_result_async() # 使用目标处理的方法请求动作结果，当动作结果出来后会返回动作结果的future
        self._get_result_future.add_done_callback(self.get_result_callback) # 通过动作结果的future给动作结果添加回调函数

    '''动作结果的回调函数'''
    def get_result_callback(self, future):
        result = future.result().result
        print(result.finish)

    '''动作周期反馈函数'''
    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        print(feedback)

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = VisualInteraction() # 创建ros2节点对象并进行初始化

    time.sleep(3) # 等待3秒后发送服务请求
    reponse = node.send_request(True) # 获取服务返回的检测结果
    print(reponse.get_result)
    
    future = node.send_goal(True) # 获取动作返回的结果

    rclpy.spin(node) # 循环等待ros2退出
    
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()